#include "radio.h"

#include <algorithm>
#include <vector>

#include "aprs.h"

// ====== 射频硬件参数 ======
static const uint32_t SAMPLE_RATE = 38400;             // ADC 采样率（Hz）
static const float BIT_RATE = 1200.0f;                 // APRS AFSK 比特率
static const float BIT_SAMPLES = (float)SAMPLE_RATE / BIT_RATE;
static const uint32_t SAMPLE_INTERVAL_US = 1000000UL / SAMPLE_RATE;
static const int16_t MIN_SIGNAL_THRESHOLD = 25;        // 静噪阈值（滤除极弱噪声）

static const int AFSK_MARK_FREQ = 1200;                // 代表逻辑“Mark”（未翻转）
static const int AFSK_SPACE_FREQ = 2200;               // 代表逻辑“Space”（发生翻转）
static const int AFSK_LEDC_CHANNEL = 0;                // PWM 通道编号
static const int AFSK_LEDC_TIMER_BITS = 10;            // PWM 分辨率

static const int PREAMBLE_FLAGS = 30;                  // 发射前导 FLAG 数量
static const int TRAILER_FLAGS = 3;                    // 发射结束 FLAG 数量

// ====== 运行时状态 ======
static int sPinSpkIn = -1;
static int sPinMicOut = -1;
static int sPinPtt = -1;
static bool sInitialized = false;

static bool sSignValid = false;
static bool sPrevSign = false;
static uint32_t sSamplesSinceCross = 0;
static bool sCurrentMark = true;
static bool sPrevSymbolMark = true;
static float sBitPhase = 0.0f;

static int32_t sDcEstimate = 0;
static bool sDcReady = false;
static int32_t sAbsEstimate = 0;

static uint32_t sLastSampleUs = 0;

// NRZI -> NRZ 解码状态
static int sOnesCount = 0;
static uint8_t sCurByte = 0;
static uint8_t sBitPos = 0;

static std::vector<uint8_t> sRxFrameBuf;
static bool sReceiving = false;
static const size_t RX_FRAME_MAX = 330;

static RadioFrame sLastFrame;
static bool sHasNewFrame = false;
static unsigned long sDecodeOk = 0;
static unsigned long sDecodeErr = 0;

// ====== 内部函数声明 ======
static void processSample(int raw);
static void handleSymbol(bool isMark);
static void handleBit(bool bit);
static void handleByte(uint8_t byte);
static bool buildAprsInfoPayload(float lat,
                                 float lon,
                                 char symbolTable,
                                 char symbolCode,
                                 const String &comment,
                                 std::vector<uint8_t> &info);

// ====== API 实现 ======
void radioInit(int pinSpkIn, int pinMicOut, int pinPtt) {
  sPinSpkIn = pinSpkIn;
  sPinMicOut = pinMicOut;
  sPinPtt = pinPtt;

  if (sPinSpkIn >= 0) {
    pinMode(sPinSpkIn, INPUT);
    analogReadResolution(12);
  }

  if (sPinMicOut >= 0) {
    pinMode(sPinMicOut, OUTPUT);
    ledcSetup(AFSK_LEDC_CHANNEL, AFSK_MARK_FREQ, AFSK_LEDC_TIMER_BITS);
    ledcAttachPin(sPinMicOut, AFSK_LEDC_CHANNEL);
    ledcWrite(AFSK_LEDC_CHANNEL, 512); // 50% 占空比
    ledcWriteTone(AFSK_LEDC_CHANNEL, 0);
  }

  if (sPinPtt >= 0) {
    pinMode(sPinPtt, OUTPUT);
    digitalWrite(sPinPtt, LOW);
  }

  sInitialized = true;
  sPrevSymbolMark = true;
  sSamplesSinceCross = 0;
  sBitPhase = 0.0f;
  sDcEstimate = 0;
  sDcReady = false;
  sAbsEstimate = 0;
  sSignValid = false;
  sReceiving = false;
  sRxFrameBuf.clear();
  sOnesCount = 0;
  sCurByte = 0;
  sBitPos = 0;
  sLastSampleUs = micros();
}

void radioHandle() {
  if (!sInitialized || sPinSpkIn < 0) return;

  uint32_t now = micros();
  while ((uint32_t)(now - sLastSampleUs) >= SAMPLE_INTERVAL_US) {
    sLastSampleUs += SAMPLE_INTERVAL_US;
    int raw = analogRead(sPinSpkIn); // 0 ~ 4095
    processSample(raw);
  }
}

bool radioSendAx25(const std::vector<uint8_t> &frame) {
  if (!sInitialized || sPinMicOut < 0 || sPinPtt < 0) return false;
  if (frame.size() < 4) return false;

  const uint32_t bitIntervalUs = 1000000UL / (uint32_t)BIT_RATE; // 833
  const uint32_t bitIntervalRem = 1000000UL - ((uint32_t)BIT_RATE * bitIntervalUs); // 400

  std::vector<uint8_t> nrzBits;
  nrzBits.reserve((PREAMBLE_FLAGS + TRAILER_FLAGS) * 8 + frame.size() * 10);

  auto appendFlagBits = [&]() {
    uint8_t flag = 0x7E;
    for (int i = 0; i < 8; ++i) {
      nrzBits.push_back((flag >> i) & 0x01);
    }
  };

  for (int i = 0; i < PREAMBLE_FLAGS; ++i) appendFlagBits();

  int onesCnt = 0;
  for (size_t idx = 0; idx < frame.size(); ++idx) {
    uint8_t byte = frame[idx];
    for (int bit = 0; bit < 8; ++bit) {
      uint8_t b = (byte >> bit) & 0x01;
      nrzBits.push_back(b);
      if (b) {
        onesCnt++;
        if (onesCnt == 5) {
          nrzBits.push_back(0);
          onesCnt = 0;
        }
      } else {
        onesCnt = 0;
      }
    }
  }

  onesCnt = 0;
  for (int i = 0; i < TRAILER_FLAGS; ++i) {
    appendFlagBits();
  }

  std::vector<uint8_t> toneSeq;
  toneSeq.reserve(nrzBits.size());
  bool mark = true;
  for (uint8_t bit : nrzBits) {
    if (bit == 0) mark = !mark;
    toneSeq.push_back(mark ? 1 : 0);
  }

  digitalWrite(sPinPtt, HIGH);
  delay(5); // PTT 上升沿预留时间

  uint32_t rem = 0;
  for (uint8_t toneBit : toneSeq) {
    ledcWriteTone(AFSK_LEDC_CHANNEL, toneBit ? AFSK_MARK_FREQ : AFSK_SPACE_FREQ);
    delayMicroseconds(bitIntervalUs);
    rem += bitIntervalRem;
    if (rem >= (uint32_t)BIT_RATE) {
      delayMicroseconds(1);
      rem -= (uint32_t)BIT_RATE;
    }
    yield();
  }

  ledcWriteTone(AFSK_LEDC_CHANNEL, 0);
  delay(5);
  digitalWrite(sPinPtt, LOW);
  return true;
}

bool radioSendAprsPosition(const String &callsign,
                           float latitude,
                           float longitude,
                           char symbolTable,
                           char symbolCode,
                           const String &comment) {
  std::vector<uint8_t> info;
  if (!buildAprsInfoPayload(latitude, longitude, symbolTable, symbolCode, comment, info))
    return false;

  std::vector<String> digis = { String(F("WIDE1-1")), String(F("WIDE2-1")) };
  std::vector<uint8_t> frame;
  if (!ax25_buildUIFrame(String(F("APRS")), callsign, digis, info, frame)) return false;
  return radioSendAx25(frame);
}

bool radioSendAprsMessage(const String &callsign,
                          const String &target,
                          const String &text,
                          const String &msgId) {
  String targetField = target;
  targetField.trim();
  targetField.toUpperCase();
  if (targetField.length() > 9) targetField = targetField.substring(0, 9);
  while (targetField.length() < 9) targetField += ' ';

  String body = ":";
  body += targetField;
  body += ":";

  String payload = text;
  if (payload.length() > 67) payload = payload.substring(0, 67);
  body += payload;

  if (msgId.length() > 0) {
    String id = msgId;
    if (id.length() > 5) id = id.substring(0, 5);
    body += "{";
    body += id;
  }

  std::vector<uint8_t> info(body.begin(), body.end());
  std::vector<String> digis = { String(F("WIDE1-1")), String(F("WIDE2-1")) };
  std::vector<uint8_t> frame;
  if (!ax25_buildUIFrame(String(F("APRS")), callsign, digis, info, frame)) return false;
  return radioSendAx25(frame);
}

bool radioFetchFrame(RadioFrame &frame) {
  if (!sHasNewFrame) return false;
  frame = sLastFrame;
  sHasNewFrame = false;
  return true;
}

String radioLastInfoText() {
  return sLastFrame.infoText;
}

unsigned long radioDecodeSuccessCount() { return sDecodeOk; }
unsigned long radioDecodeErrorCount() { return sDecodeErr; }
unsigned long radioLastInfoMillis() { return sLastFrame.rxTimeMs; }

// ====== 内部实现 ======
static void processSample(int raw) {
  if (!sDcReady) {
    sDcEstimate = raw << 8;
    sDcReady = true;
  } else {
    sDcEstimate += (((int32_t)raw << 8) - sDcEstimate) >> 6; // 1/64
  }

  int32_t centered = ((int32_t)raw << 8) - sDcEstimate;
  int16_t sample = (int16_t)(centered >> 5); // 缩放控制幅度
  int16_t absSample = sample >= 0 ? sample : (int16_t)-sample;

  sAbsEstimate += (absSample - sAbsEstimate) >> 4; // 1/16 平滑
  int16_t threshold = (int16_t)std::max<int32_t>(MIN_SIGNAL_THRESHOLD, sAbsEstimate / 3);

  bool sign = sample >= 0;
  sSamplesSinceCross++;
  if (!sSignValid) {
    sPrevSign = sign;
    sSignValid = true;
    sSamplesSinceCross = 0;
  } else if (sign != sPrevSign && absSample > threshold) {
    uint32_t interval = sSamplesSinceCross;
    sSamplesSinceCross = 0;
    sPrevSign = sign;
    if (interval > 0) {
      float periodSamples = (float)interval * 2.0f;
      float freq = SAMPLE_RATE / periodSamples;
      if (freq < 1700.0f) {
        sCurrentMark = true;
      } else {
        sCurrentMark = false;
      }
    }
  } else if (sSamplesSinceCross > (SAMPLE_RATE / 100)) {
    // 持续未翻转，认为信号停止，渐进回到 Mark
    sCurrentMark = true;
    sSamplesSinceCross = 0;
  }

  sBitPhase += 1.0f;
  if (sBitPhase >= BIT_SAMPLES) {
    sBitPhase -= BIT_SAMPLES;
    handleSymbol(sCurrentMark);
  }
}

static void handleSymbol(bool isMark) {
  bool bit = (isMark == sPrevSymbolMark);
  sPrevSymbolMark = isMark;
  handleBit(bit);
}

static void handleBit(bool bit) {
  if (bit) {
    sOnesCount++;
  } else {
    if (sOnesCount == 5) {
      sOnesCount = 0;
      return; // Stuffed 0
    }
    sOnesCount = 0;
  }

  if (bit) sCurByte |= (1 << sBitPos);
  sBitPos++;
  if (sBitPos == 8) {
    handleByte(sCurByte);
    sCurByte = 0;
    sBitPos = 0;
  }
}

static void handleByte(uint8_t byte) {
  if (byte == 0x7E) {
    if (sReceiving && sRxFrameBuf.size() >= 2) {
      uint16_t recvFcs = (uint16_t)sRxFrameBuf[sRxFrameBuf.size() - 2] |
                         ((uint16_t)sRxFrameBuf[sRxFrameBuf.size() - 1] << 8);
      sRxFrameBuf.resize(sRxFrameBuf.size() - 2);

      if (ax25_verifyFcs(sRxFrameBuf, recvFcs)) {
        Ax25Frame frame;
        if (ax25_parseFrame(sRxFrameBuf, frame)) {
          sDecodeOk++;
          frame.fcsOk = true;
          sLastFrame.ax25 = frame;
          sLastFrame.rxTimeMs = millis();
          sLastFrame.infoText = "";
          if (!frame.info.empty()) {
            for (uint8_t c : frame.info) {
              if (c >= 0x20 && c <= 0x7E) {
                sLastFrame.infoText += (char)c;
              } else if (c == '\r' || c == '\n') {
                sLastFrame.infoText += ' ';
              }
            }
          }
          sHasNewFrame = true;
        } else {
          sDecodeErr++;
        }
      } else {
        sDecodeErr++;
      }
    }
    sRxFrameBuf.clear();
    sReceiving = true;
    sOnesCount = 0;
    sCurByte = 0;
    sBitPos = 0;
    return;
  }

  if (!sReceiving) return;

  if (sRxFrameBuf.size() >= RX_FRAME_MAX) {
    sRxFrameBuf.clear();
    sReceiving = false;
    sDecodeErr++;
    return;
  }
  sRxFrameBuf.push_back(byte);
}

static bool buildAprsInfoPayload(float lat,
                                 float lon,
                                 char symbolTable,
                                 char symbolCode,
                                 const String &comment,
                                 std::vector<uint8_t> &info) {
  if (isnan(lat) || isnan(lon)) return false;
  String payload = "=";
  payload += aprs_formatLat(lat);
  payload += symbolTable;
  payload += aprs_formatLon(lon);
  payload += symbolCode;
  if (comment.length() > 0) {
    payload += " ";
    payload += comment;
  }
  info.assign(payload.begin(), payload.end());
  return true;
}
